// @(#)root/minuit2:$Id: FumiliStandardMaximumLikelihoodFCN.cxx 29198 2009-06-24 13:05:43Z brun $
// Authors: M. Winkler, F. James, L. Moneta, A. Zsenei   2003-2005  

/**********************************************************************
 *                                                                    *
 * Copyright (c) 2005 LCG ROOT Math team,  CERN/PH-SFT                *
 *                                                                    *
 **********************************************************************/


#include "Minuit2/FumiliStandardMaximumLikelihoodFCN.h"

#include <vector>
#include <cmath>
#include <float.h>

namespace ROOT {

   namespace Minuit2 {


//#include <iostream>

std::vector<double> FumiliStandardMaximumLikelihoodFCN::Elements(const std::vector<double>& par) const {
   
   //   calculate likelihood element f(i) = pdf(x(i))
   std::vector<double> result;
   double tmp1 = 0.0;
   unsigned int fPositionsSize = fPositions.size();
   
   
   for(unsigned int i=0; i < fPositionsSize; i++) {
      
      const std::vector<double> & currentPosition = fPositions[i];
      
      // The commented line is the object-oriented way to do it
      // but it is faster to do a single function call...
      //(*(this->getModelFunction())).SetParameters(par);
      tmp1 = (*(this->ModelFunction()))(par, currentPosition);
      
      // std::cout << " i = " << i << "  " << currentPosition[0] << "  " << tmp1 << std::endl; 
      
      result.push_back(tmp1);
      
   }
   
   
   return result;
   
}



const std::vector<double> & FumiliStandardMaximumLikelihoodFCN::GetMeasurement(int index) const {
   // Return x(i).
   return fPositions[index];
   
}


int FumiliStandardMaximumLikelihoodFCN::GetNumberOfMeasurements() const {
   // return size of positions (coordinates). 
   return fPositions.size();
   
}


void  FumiliStandardMaximumLikelihoodFCN::EvaluateAll( const std::vector<double> & par) { 
   // Evaluate in one loop likelihood value, gradient and hessian 
   
   static double minDouble = 8.0*DBL_MIN; 
   static double minDouble2 = std::sqrt(8.0*DBL_MIN); 
   static double maxDouble2 = 1.0/minDouble2;
   // loop on the measurements 
   
   int nmeas = GetNumberOfMeasurements(); 
   std::vector<double> & grad = Gradient();
   std::vector<double> & h = Hessian();
   int npar = par.size();
   double logLikelihood = 0; 
   grad.resize(npar);
   h.resize( static_cast<unsigned int>(0.5 * npar* (npar + 1) ) );
   grad.assign(npar, 0.0);
   h.assign(static_cast<unsigned int>(0.5 * npar* (npar + 1) ) , 0.0);
   
   const ParametricFunction & modelFunc = *ModelFunction();
   
   for (int i = 0; i < nmeas; ++i) { 
      
      // work for one-dimensional points
      const std::vector<double> & currentPosition = fPositions[i];
      modelFunc.SetParameters( currentPosition );
      double fval = modelFunc(par); 
      if (fval < minDouble ) fval = minDouble;   // to avoid getting infinity and nan's
      logLikelihood -= std::log( fval); 
      double invFval = 1.0/fval;
      // this method should return a reference
      std::vector<double> mfg = modelFunc.GetGradient(par);
      
      // calc derivatives 
      
      for (int j = 0; j < npar; ++j) { 
         if ( std::fabs(mfg[j]) < minDouble ) {
            // 	std::cout << "SMALL values: grad =  " << mfg[j] << "  "  << minDouble << " f(x) = " << fval 
            // 		  << " params " << j << " p0 = " << par[0] << " p1 = " << par[1] <<  std::endl;
            if (mfg[j] < 0)  
               mfg[j] =  -minDouble;
            else 
               mfg[j] =  minDouble;
         }	  
         
         double dfj = invFval * mfg[j]; 
         // to avoid summing infinite and nan later when calculating the Hessian
         if ( std::fabs(dfj) > maxDouble2 ) { 
            if (dfj > 0) 
               dfj = maxDouble2; 
            else 
               dfj = -maxDouble2;
         }
         
         grad[j] -= dfj;
         //       if ( ! ( dfj > 0) && ! ( dfj <= 0 ) ) 
         // 	std::cout << " nan : dfj = " << dfj << " fval =  " << fval << " invF = " << invFval << " grad = " << mfg[j] << " par[j] = " << par[j] << std::endl;  
         
         //std::cout << " x = "  << currentPosition[0] <<  " par[j] = " << par[j] << " : dfj = " << dfj << " fval =  " << fval << " invF = " << invFval << " grad = " << mfg[j] << " deriv = " << grad[j] << std::endl;  
         
         
         // in second derivative use Fumili approximation neglecting the term containing the 
         // second derivatives of the model function
         for (int k = j; k < npar; ++ k) { 
            int idx =  j + k*(k+1)/2; 
            if (std::fabs( mfg[k]) < minDouble ) { 
               if (mfg[k] < 0)  
                  mfg[k] =  -minDouble;
               else 
                  mfg[k] =  minDouble;
            }
            
            double dfk =  invFval * mfg[k]; 
            // avoid that dfk*dfj are one small and one huge so I get a nan 
            // to avoid summing infinite and nan later when calculating the Hessian
            if ( std::fabs(dfk) > maxDouble2 ) { 
               if (dfk > 0) 
                  dfk = maxDouble2; 
               else 
                  dfk = -maxDouble2;
            }
            
            
            h[idx] += dfj * dfk; 
            // 	if ( ( ! ( h[idx] > 0) && ! ( h[idx] <= 0 ) ) ) 
            // 	  std::cout << " nan : dfj = " << dfj << " fval =  " << fval << " invF = " << invFval << " gradj = " << mfg[j] 
            // 		    << " dfk = " << dfk << " gradk =  "<< mfg[k]  << " hess_jk = " << h[idx] << " par[k] = " << par[k] << std::endl;    
         }
         
      } // end param loop  
      
   } // end points loop 
   
   //   std::cout <<"\nEVALUATED GRADIENT and HESSIAN " << std::endl;
   //   for (int j = 0; j < npar; ++j) { 
   //     std::cout << " j = " << j << " grad = " << grad[j] << std::endl; 
   //     for (int k = j; k < npar; ++k) { 
   //       std::cout << " k = " << k << " hess = " << Hessian(j,k) << "  " << h[ j + k*(k+1)/2]  << std::endl; 
   //     }
   //   }
   
   // set Value in base class
   SetFCNValue( logLikelihood); 
   
}

   }  // namespace Minuit2

}  // namespace ROOT
